clc
clear all
%urdf模型导入
robot = importrobot("C:\Users\Admin\Desktop\biped_v2\xacro\leg_v2_serial_left.urdf")
% [conf,JointVel,JointAccel,t]=stepper([0.04 -1.57 -2;0.2 1.57 2],1,100)
robot.DataFormat='column';
robot.Gravity =[0 0 -9.81]%%


%定义连杆的惯量 5连杆
I_c=zeros(3,3,5);
I_o=zeros(3,3,5);
d_oc=zeros(3,5);
m=zeros(1,5);
%连杆1 link_left_hip  
m(1)=0.6262;
I_c(:,:,1)=[776019E-09 -712E-09 128330E-09;
            -712E-09   627085E-09 585E-09;
            128330E-09   585E-09  528910E-09];
d_oc(:,1)=[-0.0568 0.0001 -0.0494]'; 
%连杆2 link_left_thigh_base
m(2)=1.8264;
I_c(:,:,2)=[4741313E-09 3743E-09    -157393E-09;
            3743E-09    2405029E-09 -29199E-09;
            -157393E-09 -29199E-09  4606284E-09];
d_oc(:,2)=[0.0535 0.0259 -0.0553]';
%连杆3 link_left_thigh
m(3)=0.448;
I_c(:,:,3)=[1146E-06 -34E-06  -83E-06;
            -34E-06  1126E-06 289E-06;
            -83E-06  289E-06  560E-06];
d_oc(:,3) = [0.006764 0.018274 -0.077578]';
%连杆4 link_left_calf
m(4)=0.245;
I_c(:,:,4)=[1396.76E-06 -0.25E-06    9.327E-06;
            -0.25E-06   1442.077E-06 -0.079E-06;
            9.327E-06   -0.079E-06   102.718E-06];
d_oc(:,4) = [0.019399 -0.000164 -0.12196]';     
%连杆5 link_left_foot
m(5)=0.131856;
I_c(:,:,5)=[53406E-09   0          -30266E-09;
            0           167145E-09 0.447E-09;
            -30266E-09  0.447E-09  137711E-09]; 
d_oc(:,5) = [0.025662 0 -0.042076]';   

for i=1:1:5
    I_o(:,:,i)=I_c(:,:,i)+m(i)*skew_matrix(d_oc(:,i))*skew_matrix(d_oc(:,i))';
end

%模型得到的动力学参数集合 param =[mi micx micy micz i_xx i_xy i_xz i_yy i_yz i_zz]
param_model_matrix=zeros(10,5);
for i=1:5
    param_model_matrix(1,i)=m(i);
    param_model_matrix(2:4,i)=m(i)*d_oc(:,i);
    param_model_matrix(5,i)=I_o(1,1,i);
    param_model_matrix(6,i)=I_o(1,2,i);
    param_model_matrix(7,i)=I_o(1,3,i);
    param_model_matrix(8,i)=I_o(2,2,i);
    param_model_matrix(9,i)=I_o(2,3,i);
    param_model_matrix(10,i)=I_o(3,3,i);
%%
%     param_model_matrix(5,i)=robot.Bodies{1,i+1}.Inertia(1);
%     param_model_matrix(6,i)=robot.Bodies{1,i+1}.Inertia(4);
%     param_model_matrix(7,i)=robot.Bodies{1,i+1}.Inertia(5);
%     param_model_matrix(8,i)=robot.Bodies{1,i+1}.Inertia(2);
%     param_model_matrix(9,i)=robot.Bodies{1,i+1}.Inertia(6);
%     param_model_matrix(10,i)=robot.Bodies{1,i+1}.Inertia(3);
end
Para_vec=zeros(50,1);
Para_vec =[param_model_matrix(:,1);
           param_model_matrix(:,2);
           param_model_matrix(:,3);
           param_model_matrix(:,4);
           param_model_matrix(:,5)];

tau=zeros(5,1000);
tau_ideal=zeros(5,1000);
for i=1:1:1000
    for j = 1:5
        q(j,i) = pi/6*sin(2*pi*i/1000);
        dq(j,i) =pi/3*pi*cos(2*pi*i/1000);
        ddq(j,i) =-2*pi/3*pi*pi*sin(2*pi*i/1000);
        q(j,i) = 0;
    end
    
    
    n = 5;
    %%机构学参数定义
    %关节有J1绕（z1轴） J2绕（x2轴） J3绕（y3轴） J4绕（y4轴） J5绕（y5轴）
    %连杆link1 link2 link3 link4 link5 
    %向量P 3*1*5 为第i个坐标系相对于i-1个坐标系的位置向量，但i=0为基坐标系  i=5为脚踝坐标系
    P=zeros(3,5);
%     P(:,1)=[0,0,0]';
    P(:,1)=[0.01,0.11,0.002]';
    P(:,2)=[-0.0424,0,-0.055]';
    P(:,3)=[0.055,0,-0.06]';
    P(:,4)=[0,0,-0.18]';
    P(:,5)=[0,0,-0.24]';
    %初始关节角度theta0
    q0=zeros(5,1);
    q0(5)=1.309;
    q=q+q0;
    %旋转轴v_axis 3*1*5
    v_axis=zeros(3,5);
    v_axis(:,1)=[0;0;1];%关节1为hip yaw 绕z1轴旋转
    v_axis(:,2)=[1;0;0];%关节2为hip raw 绕x2轴旋转
    v_axis(:,3)=[0;1;0];%关节3为hip pitch 绕y3轴旋转
    v_axis(:,4)=[0;1;0];%关节1为knee yaw 绕y4轴旋转
    v_axis(:,5)=[0;1;0];%关节1为ankle yaw 绕y5轴旋转

    %构建各坐标系齐次变换矩阵、旋转矩阵
    T=zeros(4,4,n);R=zeros(3,3,n);%P=zeros(3,n);
    %%关节1
    for i = 1:1:5
        R(:,:,i)=Rotx(v_axis(1,i)*q(i))*Roty(v_axis(2,i)*q(i))*Rotz(v_axis(3,i)*q(i));
        T(:,:,i)=[R(:,:,i)   P(:,i);
                  zeros(1,3)    1  ];
    end
    T_end = T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)
    
    q_imput = q-[0;0;0;0;1.309];
    T_ideal=getTransform(robot, q_imput ,robot.BodyNames{1,6},robot.BodyNames{1,1})

    %转换矩阵建立
    %运动学正向递推
    w0 = zeros(3,1); dw0 = zeros(3,1);
    g=9.81;
    % dv0 = [0;0;g];
    dv0 = [0;0;g];
    w = zeros(3,n); dw = zeros(3,n);
    dv = zeros(3,n);

    %i = 0 计算角速度omega1=d1 角加速度alpha1=dw1  加速度a1=dv1
    w(:,1) = R(:,:,1)' * w0 + dq(1) * v_axis(:,1);
    dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dq(1) * v_axis(:,1)) + ddq(1) * v_axis(:,1);
    dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
    for i = 1:n-1
       w(:,i+1) = R(:,:,i+1)' * w(:,i) + dq(i+1) * v_axis(:,i+1) ;
       dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dq(i+1) * v_axis(:,i+1))+ ddq(i+1) * v_axis(:,i+1);
       dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
    end
%     [U,Uj] = Compute_LinearDynmatrix(q(:,i), dq(:,i), ddq(:,i));
%     tau_temp= Uj *Para_vec;
%     tau_ideal_temp = inverseDynamics(robot,q(:,i),dq(:,i),ddq(:,i));
%     tau(:,i)=tau_temp;
%     tau_ideal(:,i)=tau_ideal_temp;
    if i == 200
        c=1
    end
end

figure(1)
for j=1:5
    subplot(2,3,j)
    t=0.001:0.001:1
    plot(t,tau(j,:),'-r')
    
    hold on
    plot(t,tau_ideal(j,:),'--g')
    
    xlabel('t(s)');
    ylabel('joint torque(Nm)')
end




